#include "StdAfx.h"
#include "SpeedSensor.h"



SpeedSensor::~SpeedSensor(void)
{
delete hrt;
}

SpeedSensor::SpeedSensor(double kt, double r, double m, double _b)
{
	hrt = new HiResTimer();
	Kt = kt;
	R = r;
	M = m;
	b = _b;
	J = (double)0.5 * M * R * R;
	A = -(Kt * Kt) / (J * R) - b / J;
	B = Kt / (J * R);
}


double SpeedSensor::GetCurrentSpeed(double voltage)
{
	//dt = time - p_T;
	//p_T = time;
	
	dt = hrt->Stop();
	p_W = (p_W+((A*p_W)+(B*voltage)*dt));
	hrt->Start();
	return p_W;
}